#include "ellipse_det_node.h"
#include <cmath>
#include <opencv2/calib3d.hpp>
#include <opencv2/dnn.hpp>
#include <fstream>


namespace sv2 {

using namespace cv;
using namespace cv::dnn;


void EllipseDetCppNode::run()
{
  while (this->is_running())
  {
    // 主线程阻塞，等待队里中出现数据
    cv::Mat image;
    if (!this->_image_queue.empty())
    {
      {
        std::unique_lock<std::mutex> lock(this->_image_queue_mtx);
        ImgMsg imgmsg = this->_image_queue.front();
        imgmsg.img.copyTo(image);
        this->_image_queue.pop();
      }

      float fMaxCenterDistance = sqrt(float(image.cols*image.cols + image.rows*image.rows)) * this->_max_center_distance;
      _ed->SetMCD(fMaxCenterDistance);
      std::vector<yaed::Ellipse> ellsCned;
      _ed->Detect(image, ellsCned);

      nlohmann::json res_msg = _publish_to_sms(ellsCned, image.cols, image.rows);
      _ed->DrawDetectedEllipses(image, ellsCned);
      nlohmann::json vis_msg = _vis_pub.cvimg2sms_mem(image);
      vis_msg["spirecv_msgs::2DTargets"] = res_msg;
      _vis_pub.publish(vis_msg);
    }
  }
}



nlohmann::json EllipseDetCppNode::_publish_to_sms(std::vector<yaed::Ellipse>& ellsCned, int img_w_, int img_h_)
{
  nlohmann::json msg = sms::def_msg("spirecv_msgs::2DTargets");
  msg["width"] = img_w_;
  msg["height"] = img_h_;
  msg["fov_x"] = this->fov_x;
  msg["fov_y"] = this->fov_y;
  if (this->_t0 > 0)
  {
    msg["fps"] = 1.0 / (sms::get_time_sec() - this->_t0 + 1e-6);
  }
  else
  {
    msg["fps"] = 0;
  }
  this->_t0 = sms::get_time_sec();

  msg["targets"] = nlohmann::json::array();
  for (int i=0; i<ellsCned.size(); i++)
  {
    double xc_ = ellsCned[i].xc_;
    double yc_ = ellsCned[i].yc_;
    double a_ = ellsCned[i].a_;
    double b_ = ellsCned[i].b_;
    double rad_ = ellsCned[i].rad_;
    double score_ = ellsCned[i].score_;

    nlohmann::json target;
    target["score"] = score_;
    target["category_name"] = "ellipse";
    target["category_id"] = 0;
    cv::Rect rect;
    yaed::Ellipse ell(xc_, yc_, a_, b_, rad_);
    ell.GetRectangle(rect);
    target["bbox"] = {rect.x, rect.y, rect.width, rect.height};

    double cx = (double)(rect.x + rect.width / 2.) / img_w_;
    double cy = (double)(rect.y + rect.height / 2.) / img_h_;
    double los_ax = atan((cx * img_w_ - img_w_ / 2.) / this->camera_matrix.at<double>(0, 0)) * SV2_RAD2DEG;
    double los_ay = atan((cy * img_h_ - img_h_ / 2.) / this->camera_matrix.at<double>(1, 1)) * SV2_RAD2DEG;
    target["los"] = {los_ax, los_ay};
    target["ellipse"] = {xc_, yc_, a_, b_, rad_};

    if (this->_radius_in_meter > 0)
    {
      double z = this->camera_matrix.at<double>(0, 0) * this->_radius_in_meter / b_;
      double x = tan(los_ax / SV2_RAD2DEG) * z;
      double y = tan(los_ay / SV2_RAD2DEG) * z;
      target["position"] = {x, y, z};
    }
    msg["targets"].push_back(target);
  }
  this->_res_pub.publish(msg);
  return msg;
}




}
